#include <stdio.h>
#include <fstream>

#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <QPushButton>
#include <QFrame>

#include"rviz_path_panel.h"

namespace path_creator_rviz_plugin
{
    rvizPathPanel::rvizPathPanel(QWidget* parent) : rviz::Panel(parent)
    {
        path_to_file = "";
        frame_id = "map";
        // width of each path on the map
        line_width = 0.05;
        current_path_id = -1;
        map_id = -1;
        step_for_interpolation = 0.05;
        current_path.header.frame_id = frame_id;
        
        // Subscribers
        pose_received = nh_.subscribe("/mbf_actions/pose_for_path", 2, &rvizPathPanel::receivedNewPose, this);
        // Publishers
        // TODO: delete last_path_pub, it needs only for debug
        last_path_pub = nh_.advertise<path_creator_rviz_plugin::my_path>("/mbf_actions/path_saved", 1);
        path_creating_pub = nh_.advertise<nav_msgs::Path>("/mbf_actions/path_creating", 5);
        markers_pub = nh_.advertise<visualization_msgs::MarkerArray>("/mbf_actions/paths", 5);        
        // Clients TODO: set server name & type of msgs
        load_paths_client = nh_.serviceClient<path_creator_rviz_plugin::LoadPaths>("courier_file_server");
        save_paths_client = nh_.serviceClient<path_creator_rviz_plugin::SavePaths>("courier_file_server");

        // Layout with "Path to file: "
        QHBoxLayout* path_to_file_layout = new QHBoxLayout;
        // Layout with buttons, which response with file
        QHBoxLayout* file_buttons_layout = new QHBoxLayout;
        // Layout with "Path id:"
        QHBoxLayout* path_id_layout = new QHBoxLayout;
        // Layout with "Path description:"
        QHBoxLayout* path_description_layout = new QHBoxLayout;
        // Layout with buttons, which response with paths
        QHBoxLayout* path_buttons_layout = new QHBoxLayout;

        // One-line text editor for entering the path to file
        path_to_file_editor = new QLineEdit;
        // One-line text editor for entering the path to file
        path_id_editor = new QLineEdit;
        // One-line text editor for entering the path to file
        path_description_editor = new QLineEdit;

        // All Buttons
        show_all_paths_button = new QPushButton("show paths");
        remove_path_by_id_button = new QPushButton("Remove by id");
        create_new_path_button = new QPushButton("create new path");
        save_path_button = new QPushButton("save path");

        // All labels on layouts
        path_id_layout->addWidget( new QLabel("Path id:"));
        path_to_file_layout->addWidget( new QLabel("Map id:"));
        path_description_layout->addWidget( new QLabel("Description"));

        // Line editors on layouts
        path_to_file_layout->addWidget(path_to_file_editor);
        path_id_layout->addWidget(path_id_editor);
        path_description_layout->addWidget(path_description_editor);

        // All buttons on layouts
        file_buttons_layout->addWidget(show_all_paths_button);
        path_buttons_layout->addWidget(remove_path_by_id_button);
        path_buttons_layout->addWidget(create_new_path_button);
        path_buttons_layout->addWidget(save_path_button);

        show_all_paths_button->setToolTip("show all paths from file");
        remove_path_by_id_button->setToolTip("type id into the form and press the button");
        create_new_path_button->setToolTip("create new path");
        save_path_button->setToolTip("save path to file");

        // One layout to control all of them
        QVBoxLayout* layout = new QVBoxLayout;
        layout->addLayout(path_to_file_layout); 
        layout->addLayout(file_buttons_layout);
        layout->addLayout(path_id_layout);
        layout->addLayout(path_description_layout);
        layout->addLayout(path_buttons_layout);
        
        setLayout(layout);

        // connect Editors
        connect(path_to_file_editor, SIGNAL( editingFinished()), this, SLOT(updatePathToFile()));
        connect(path_id_editor, SIGNAL( editingFinished()), this, SLOT(updatePathId()));        
        connect(path_description_editor, SIGNAL( editingFinished()), this, SLOT(updatePathDescription()));        

        // connect Buttons
        connect(remove_path_by_id_button, SIGNAL( clicked(bool)), this, SLOT( removePathById(bool)));
        connect(save_path_button, SIGNAL( clicked(bool)), this, SLOT( savePathToFile(bool)));
        connect(create_new_path_button, SIGNAL( clicked(bool)), this, SLOT( createNewPath(bool)));
        connect(show_all_paths_button, SIGNAL( clicked(bool)), this, SLOT( showAllPaths(bool)));
    }

    // Button
    void rvizPathPanel::savePathToFile (bool trig){
        ROS_INFO("you clicked on save_paths_to_file_button");
        if (current_path.poses.size() < 2) {
            ROS_ERROR("Failed to save path. It contains less than 2 points!");
            return ;
        }
        if (current_path_id == -1 || pathIsAlreadyExist(current_path_id)) {
            ROS_ERROR("Failed to save path. Path id is incorrect!");
            return ;
        }
        if (map_id < 0){
            ROS_ERROR("Map id is incorrect!");
            return ;
        }
        
        path_creator_rviz_plugin::my_path my_path;
        my_path.path_id = current_path_id;
        my_path.description = path_description;
        my_path.path = current_path;
        my_path.map_id = map_id;
        last_path_pub.publish(my_path);
        paths_list.push_back(my_path);

        // clear previous path from RVIZ window
        current_path.poses.clear();
        path_creating_pub.publish(current_path);

        showAllPaths(true);

        // TODO: client is sending request to save file
        path_creator_rviz_plugin::SavePaths save_paths_srv;
        std::vector<path_creator_rviz_plugin::my_path> my_paths_vector (paths_list.begin(), paths_list.end());
        save_paths_srv.request.paths =  my_paths_vector;
        save_paths_srv.request.map_id = map_id;
        if (save_paths_client.call(save_paths_srv)){
            if (save_paths_srv.response.is_ok){
                ROS_INFO("File with paths was saved");
            }
            else {
                ROS_ERROR("Failed to save paths. Error on the side of server");
            }
        }
        else{
            ROS_ERROR("Failed to call service courier_file_server");
        }
    }

    // Button
    void rvizPathPanel::showAllPaths (bool trig){
        ROS_INFO("you clicked on show_all_paths_button");
        //clear screen
        clearAllMarkers();
        displayAllPaths(marker_array_paths);
        // display markers
        marker_array_paths = pathsToMarkersArray(paths_list);
        displayAllPaths(marker_array_paths);
    }

    // Button
    void rvizPathPanel::displayAllPaths(visualization_msgs::MarkerArray marker_array_){
        // publish marker array
        ROS_INFO("Display all paths");
        markers_pub.publish(marker_array_);
    }

    // Button
    void rvizPathPanel::removePathById (bool trig){
        ROS_INFO("you clicked on remove path by id button");
        removePathById(current_path_id);
        showAllPaths(true);

    }

    // Button 
    void rvizPathPanel::createNewPath (bool trig){
        ROS_INFO("you clicked on create_new_path_button");
        current_path.poses.clear();
        path_creating_pub.publish(current_path);
    }

    // Editor path id
    void rvizPathPanel::updatePathId(){
        QString new_id = path_id_editor->text();
        if (new_id != ""){
            // TODO: check new_id contains only numbers! 
            current_path_id = new_id.toInt();
        }
        else {
            std::cout<<"Empty id... Enter the number" << std::endl;            
        }
    }

    // Editor Path to file
    void rvizPathPanel::updatePathToFile(){
        QString new_path = path_to_file_editor->text();
        if (new_path != ""){
            // TODO: update transform path to map_id
            path_to_file = new_path.toStdString();
            map_id = std::stoi( path_to_file );
            std::cout<<"Path to file is modified to: " << path_to_file << std::endl;
            loadPaths(std::stoi( path_to_file ));
        }
        else {
            ROS_ERROR("Empty path... Enter correct path!");
        }
    }

    // Editor path description
    void rvizPathPanel::updatePathDescription(){
        QString new_description = path_description_editor->text();
        if (new_description != ""){
            path_description = new_description.toStdString();
        }
        else{
            ROS_INFO("Empty description!");
            path_description = new_description.toStdString();
        }
    }

    // method to clear all markers
    void rvizPathPanel::clearAllMarkers(){
        int length = marker_array_paths.markers.size();
        for (int i = 0; i < length; i++){
            marker_array_paths.markers[i].action = visualization_msgs::Marker::DELETE;
        }
    }

    // 
    visualization_msgs::MarkerArray rvizPathPanel::pathsToMarkersArray(std::list<path_creator_rviz_plugin::my_path> paths_list_) {
        visualization_msgs::MarkerArray marker_array;
        
        if (paths_list_.size() == 0){
            ROS_ERROR("Empty path list!");
            return marker_array;
        }

        int i = 0;
        for (std::list<path_creator_rviz_plugin::my_path>::iterator it = paths_list_.begin(); it != paths_list_.end(); it++){
            path_creator_rviz_plugin::my_path my_path = *it;
            // add path_marker to list of markers
            visualization_msgs::Marker path_marker = transformPathToMarker(my_path.path);
            // add id to list of markers
            visualization_msgs::Marker id_marker = transformMyPathToIdLabelMarker(my_path.path.poses.back(), my_path.path_id);
            path_marker.id = i;
            id_marker.id = i+1;
            marker_array.markers.push_back(path_marker);
            marker_array.markers.push_back(id_marker);

            i += 2;
        }
        return marker_array;
    }

    //transforming path to marker
    visualization_msgs::Marker rvizPathPanel::transformPathToMarker(nav_msgs::Path path){
        visualization_msgs::Marker marker;
        marker.type = visualization_msgs::Marker::LINE_STRIP;
        marker.action = visualization_msgs::Marker::ADD;
        marker.ns = "my_namespace";
        marker.scale.x = line_width;
        marker.scale.y = line_width;
        marker.scale.z = line_width;
        marker.color.a = 1.0; // Don't forget to set the alpha!
        marker.color.g = 1.0;
        marker.header.stamp = ros::Time::now();
        marker.lifetime = ros::Duration();
        marker.header.frame_id = path.header.frame_id;

        for (std::vector<geometry_msgs::PoseStamped>::iterator it = path.poses.begin(); it != path.poses.end(); it++){
            geometry_msgs::Point point;
            geometry_msgs::PoseStamped pose_stamped = *it;
            point.x = pose_stamped.pose.position.x;
            point.y = pose_stamped.pose.position.y;
            point.z = pose_stamped.pose.position.z;
            marker.points.push_back(point);
        }
        return marker;
    }

    // transforming my_path to id marker 
    visualization_msgs::Marker rvizPathPanel::transformMyPathToIdLabelMarker(geometry_msgs::PoseStamped pose_stamped, int path_id){
        visualization_msgs::Marker marker;
        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        marker.action = visualization_msgs::Marker::ADD;
        marker.ns = "my_namespace";
        marker.scale.z = 0.45;
        marker.pose.position = pose_stamped.pose.position;
        marker.pose.orientation.w = 1;
        marker.text = "id: " + std::to_string( path_id );
        marker.header.stamp = ros::Time::now();
        marker.lifetime = ros::Duration();
        marker.header.frame_id = frame_id;
        marker.color.r = 1.0;
        marker.color.a = 10.0;
        return marker;
    }


    // Subscriber on the Pose tool
    void rvizPathPanel::receivedNewPose(const geometry_msgs::Pose::ConstPtr& msg){
        ROS_INFO("Pose was received");

        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.frame_id = frame_id;
        pose_stamped.pose.position = msg->position;
        pose_stamped.pose.orientation = msg->orientation;

        if (current_path.poses.size() >= 1){
            // poses = (pose(-1), .... , pose_stamped]
            std::vector<geometry_msgs::PoseStamped> poses;

            poses = interpolationBetweenTwoPoses(current_path.poses.back(), pose_stamped);
            for (std::vector<geometry_msgs::PoseStamped>::iterator it = poses.begin(); it != poses.end(); it++){
                current_path.poses.push_back(*it);
            }
        }
        else{
            current_path.poses.push_back(pose_stamped);
        }
        path_creating_pub.publish(current_path);
    }

    // Load paths by map_id
    void rvizPathPanel::loadPaths(int map_id_){
        path_creator_rviz_plugin::LoadPaths srv;
        // TODO: delete next string 
        map_id = map_id_;
        srv.request.map_id = map_id_;
        if (load_paths_client.call(srv)){
            if (srv.response.is_ok){
                map_id = map_id_;
                ROS_INFO("map_id changed");
            }
            else{
                ROS_ERROR("map_id can't be changed");
            }
        }
        else {
            ROS_ERROR("Failed to call service to load paths");
        }
    }

    // return poses = (pose_prev, .... , pose_new]
    std::vector<geometry_msgs::PoseStamped> rvizPathPanel::interpolationBetweenTwoPoses(geometry_msgs::PoseStamped pose_prev, geometry_msgs::PoseStamped pose_new){
        std::vector<geometry_msgs::PoseStamped> poses;
        float distance = std::hypot(pose_prev.pose.position.x - pose_new.pose.position.x, pose_prev.pose.position.y - pose_new.pose.position.y);
        float yaw = angleBetweenPoints(pose_prev.pose, pose_new.pose);

        geometry_msgs::Quaternion quat_msg = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, yaw);

        int N = (int) (distance / step_for_interpolation);
        if (N < 2){
            poses.push_back(pose_new);
            return poses;
        }

        float dx = (pose_new.pose.position.x - pose_prev.pose.position.x) / (N - 1);
        float dy = (pose_new.pose.position.y - pose_prev.pose.position.y) / (N - 1);
        for (int i = 1; i < N - 1; i++){
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.pose.position.x = pose_prev.pose.position.x + dx * i;
            pose_stamped.pose.position.y = pose_prev.pose.position.y + dy * i;
            pose_stamped.pose.position.z = 0;
            pose_stamped.pose.orientation = quat_msg;
            pose_stamped.header.frame_id = frame_id;
            poses.push_back(pose_stamped);
        }
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.pose.position.x = pose_prev.pose.position.x + dx * N;
        pose_stamped.pose.position.y = pose_prev.pose.position.y + dy * N;
        pose_stamped.pose.position.z = 0;
        pose_stamped.pose.orientation = pose_new.pose.orientation;
        pose_stamped.header.frame_id = frame_id;
        poses.push_back(pose_stamped);

        return poses;
    }

    // return list of paths, which don't include path with path_id
    void rvizPathPanel::removePathById(int path_id){
        for (std::list<path_creator_rviz_plugin::my_path>::iterator it = paths_list.begin(); it != paths_list.end(); it++){
            path_creator_rviz_plugin::my_path temp_path = *it;
            if (temp_path.path_id == path_id){
                paths_list.erase(it);
                ROS_INFO("Path was removed");
                return;
            }
        }
        ROS_INFO("Warning! Cannot find path with setted id");
    }

    // check if path with setted id is already exist on the map 
    bool rvizPathPanel::pathIsAlreadyExist(int path_id){
        for (std::list<path_creator_rviz_plugin::my_path>::iterator it = paths_list.begin(); it != paths_list.end(); it++){
            path_creator_rviz_plugin::my_path temp_path = *it;
            if (temp_path.path_id == path_id){
                return true;
            }
        }
        return false;
    }

    // check if path with this description is already exist on the map 
    bool rvizPathPanel::pathIsAlreadyExist(std::string description){
        for (std::list<path_creator_rviz_plugin::my_path>::iterator it = paths_list.begin(); it != paths_list.end(); it++){
            path_creator_rviz_plugin::my_path temp_path = *it;
            if (temp_path.description == description){
                return true;
            }
        }
        return false;
    }

    //return yaw (angle) of vector between two poses
    float rvizPathPanel::angleBetweenPoints(geometry_msgs::Pose pose_prev, geometry_msgs::Pose pose_new){ 
        float dx = pose_new.position.x - pose_prev.position.x;
        float dy = pose_new.position.y - pose_prev.position.y;
        if (dx > 0){
            return atan(dy / dx);
        }
        if (dx < 0 && dy >= 0){
            return atan(dy / dx) + PI;
        }
        if (dx < 0 && dy < 0){
            return atan(dy/dx) - PI;
        }
        if (dx == 0 && dy > 0){
            return PI/2;
        }
        if (dx == 0 && dy < 0){
            return -PI/2;
        }
        return 0;
    }
}

// Tell pluginlib about this class.  Every class which should be
// loadable by pluginlib::ClassLoader must have these two lines
// compiled in its .cpp file, outside of any namespace scope.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(path_creator_rviz_plugin::rvizPathPanel, rviz::Panel )